/***************************************************************************//**
 * @file MSc_FDM.cpp
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 ******************************************************************************/
#ifndef MSC_FDM_CPP
#define MSC_FDM_CPP
#endif

////////////////////////////////////////////////////////////////////////////////

#include <MSc_FDM.h>

////////////////////////////////////////////////////////////////////////////////

using namespace std;

////////////////////////////////////////////////////////////////////////////////

MSc_FDM::MSc_FDM() :
    MSc_Base ( m_rootNode = new MSc_DataNode() ),

    m_aero ( 0 ),
    m_ctrl ( 0 ),
    m_gear ( 0 ),
    m_mass ( 0 ),
    m_prop ( 0 ),

    m_integrator ( 0 ),

    m_ground ( 0 ),

    m_crash ( NoCrash ),

    m_initialized ( false ),

    m_realTime ( 0.0 ),
    m_timeStep ( 0.0 )
{
    try
    {
        m_aero = new MSc_Aerodynamics ( m_rootNode );
        m_ctrl = new MSc_Controls     ( m_rootNode );
        m_gear = new MSc_LandingGear  ( m_rootNode );
        m_mass = new MSc_MassBalance  ( m_rootNode );
        m_prop = new MSc_Propulsion   ( m_rootNode );

        m_integrator = new MSc_FDM::Integrator( MSCFDM_STATE_DIMENSION, this, &MSc_FDM::computeStateDeriv );
    }
    catch ( MSc_Exception &catched )
    {
        MSc_Exception e;

        e.setCause( catched );

        e.setType( MSc_Exception::UnknownException );
        e.setFile( __FILE__ );
        e.setLine( __LINE__ );
        e.setInfo( "Exception catched." );

        throw e;
    }

    for ( int i = 0; i < MSCFDM_STATE_DIMENSION; i++ )
    {
        m_stateVect [ i ] = 0.0;
        m_statePrev [ i ] = 0.0;
        m_derivVect [ i ] = 0.0;
    }
}

////////////////////////////////////////////////////////////////////////////////

MSc_FDM::~MSc_FDM()
{
    if ( m_integrator ) delete m_integrator; m_integrator = 0;

    if ( m_aero ) delete m_aero; m_aero = 0;
    if ( m_ctrl ) delete m_ctrl; m_ctrl = 0;
    if ( m_gear ) delete m_gear; m_gear = 0;
    if ( m_mass ) delete m_mass; m_mass = 0;
    if ( m_prop ) delete m_prop; m_prop = 0;

    ////////////////////////////////////////////////////
    if ( m_rootNode ) delete m_rootNode; m_rootNode = 0;
    ////////////////////////////////////////////////////
}

////////////////////////////////////////////////////////////////////////////////

void MSc_FDM::initialize( std::string /*configFile*/, MSc_Ground *ground )
{
    m_ground = ground;

    if ( m_ground == NULL )
    {
        MSc_Exception e;

        e.setType( MSc_Exception::NullPointer );
        e.setFile( __FILE__ );
        e.setLine( __LINE__ );
        e.setInfo( "m_ground == NULL" );

        throw e;
    }

    ///////////////
    analyseModel();
    ///////////////

    m_initialized = true;
}

////////////////////////////////////////////////////////////////////////////////

void MSc_FDM::update( double timeStep )
{
    if ( m_initialized )
    {
        if ( m_crash != NoCrash ) return;

        m_timeStep = timeStep;

        try
        {
            anteIntegration();

            ///////////////////////////////////////////////////
            m_integrator->integrate( m_timeStep, m_stateVect );
            ///////////////////////////////////////////////////

            postIntegration();
        }
        catch ( MSc_Exception &catched )
        {
            MSc_Exception e;

            e.setCause( catched );

            e.setType( MSc_Exception::UnknownException );
            e.setFile( __FILE__ );
            e.setLine( __LINE__ );
            e.setInfo( "Exception catched." );

            throw e;
        }
    }
    else
    {
        MSc_Exception e;

        e.setType( MSc_Exception::UnknownException );
        e.setFile( __FILE__ );
        e.setLine( __LINE__ );
        e.setInfo( "Flight has not been initialized." );

        throw e;
    }
}

////////////////////////////////////////////////////////////////////////////////

void MSc_FDM::analyseModel() {}

////////////////////////////////////////////////////////////////////////////////

void MSc_FDM::anteIntegration() {}

////////////////////////////////////////////////////////////////////////////////

void MSc_FDM::postIntegration()
{
    if ( MSc_Utils::isValid( m_stateVect, MSCFDM_STATE_DIMENSION ) )
    {
        m_realTime += m_timeStep;

        for ( int i = 0; i < MSCFDM_STATE_DIMENSION; i++ )
        {
            m_derivVect[ i ] = ( m_stateVect[ i ] - m_statePrev[ i ] ) / m_timeStep;
            m_statePrev[ i ] = m_stateVect[ i ];
        }

        updateStateData( m_stateVect );
    }
    else
    {
        MSc_Exception e;

        e.setType( MSc_Exception::UnexpectedNaN );
        e.setFile( __FILE__ );
        e.setLine( __LINE__ );
        e.setInfo( "NaN detected." );

        throw e;
    }
}

////////////////////////////////////////////////////////////////////////////////

void MSc_FDM::computeStateDeriv( const double *stateVect, double *derivVect )
{
    updateStateData( stateVect );

    // COMPUTING FORCES AND MOMENTS
    m_aero->computeForceAndMoment();
    m_gear->computeForceAndMoment();
    m_mass->computeForceAndMoment();
    m_prop->computeForceAndMoment();

    MSc_Vector3 for_BAS = m_aero->getFor_BAS()
                        + m_mass->getFor_BAS()
                        + m_gear->getFor_BAS()
                        + m_prop->getFor_BAS();

    MSc_Vector3 mom_BAS = m_aero->getMom_BAS()
                        + m_mass->getMom_BAS()
                        + m_gear->getMom_BAS()
                        + m_prop->getMom_BAS();

    MSc_Vector3 vel_WGS = m_T_bas2wgs * m_vel_BAS;

    // COMPUTING LOCATION DERIVATIVES
    derivVect[ StateX ] = vel_WGS( U );
    derivVect[ StateY ] = vel_WGS( V );
    derivVect[ StateZ ] = vel_WGS( W );

    // COMPUTING ATTITUDE ANGLES DERIVATIVES
    double sinPhi = sin( stateVect[ StatePhi ] );
    double cosPhi = cos( stateVect[ StatePhi ] );
    double cosTht = cos( stateVect[ StateTht ] );

    derivVect[ StateTht ] = cosPhi * m_omg_BAS( Q ) - sinPhi * m_omg_BAS( R );

    // gimbal lock safety
    if ( fabs( cosTht ) > 1.0e-14 )
    {
        double tanTht = tan( stateVect[ StateTht ] );

        derivVect[ StatePhi ] = m_omg_BAS( P ) + sinPhi*tanTht * m_omg_BAS( Q ) + cosPhi*tanTht * m_omg_BAS( R );
        derivVect[ StatePsi ] =                  sinPhi/cosTht * m_omg_BAS( Q ) + cosPhi/cosTht * m_omg_BAS( R );
    }
    else
    {
        derivVect[ StatePhi ] = m_omg_BAS( P );
        derivVect[ StatePsi ] = 0.0;
    }

    // COMPUTING LINEAR AND ANGULAR VELOCITIES DERIVATIVES (EQUATIONS OF MOTION)

    // creating Right-Hand-Side vectors
    MSc_Vector3 for_RHS;
    MSc_Vector3 mom_RHS;

    double mass = 0.0;

    MSc_Vector3 cg_BAS;
    MSc_Vector3 st_BAS;

    MSc_Matrix3x3 ji_BAS;

    for_RHS = for_BAS
            - m_omg_BAS % ( mass * m_vel_BAS )
            - m_omg_BAS % ( m_omg_BAS % st_BAS );

    mom_RHS = mom_BAS
            - cg_BAS % ( m_omg_BAS % ( mass * m_vel_BAS ) )
            - m_omg_BAS % ( ji_BAS * m_omg_BAS );

/*
    // creating Right-Hand-Side assembled vector
    double rhs[ 6 ];

    rhs[ 0 ] = for_RHS( X );
    rhs[ 1 ] = for_RHS( Y );
    rhs[ 2 ] = for_RHS( Z );
    rhs[ 3 ] = mom_RHS( L );
    rhs[ 4 ] = mom_RHS( M );
    rhs[ 5 ] = mom_RHS( N );

    // state derivatives (results)
    double acc_BAS[  6 ];
    double mtr_BAS[ 36 ];

    MSc_Matrix6x6 mi_BAS;

    mi_BAS.getArray( mtr_BAS );

    // solving equations of motion: [Mi]*[S_dot]=[F]
    // TODO

    // Coriolis acceleration due to Earth rotation
    {
        // current acceleration
        MSc_Vector3 accTmp_BAS = MSc_Vector3::getFrom( acc_BAS[ 0 ], acc_BAS[ 1 ], acc_BAS[ 2 ] );

        // increasing accelerations due to Coriolis acceleration
        accTmp_BAS += - 2.0 * m_T_wgs2bas * MSc_Location::getOmega_WGS() % m_vel_BAS;

        acc_BAS[ 0 ] = accTmp_BAS( 0 );
        acc_BAS[ 1 ] = accTmp_BAS( 1 );
        acc_BAS[ 2 ] = accTmp_BAS( 2 );
    }

    // rewriting acceleration into state derivatives vector
    for ( int i = 0; i < 6; i++ ) derivVect[ i + 6 ] = acc_BAS[ i ];
*/
}

////////////////////////////////////////////////////////////////////////////////

void MSc_FDM::updateStateData( const double *stateVect )
{
    m_loc_WGS.set( stateVect[ StateX ],
                   stateVect[ StateY ],
                   stateVect[ StateZ ] );

//    m_att_WGS.set( stateVect[ StatePhi ],
//                   stateVect[ StateTht ],
//                   stateVect[ StatePsi ] );

    m_vel_BAS.set( stateVect[ StateU ],
                   stateVect[ StateV ],
                   stateVect[ StateW ] );

    m_omg_BAS.set( stateVect[ StateP ],
                   stateVect[ StateQ ],
                   stateVect[ StateR ] );

    m_location.setWGS( m_loc_WGS );

    m_T_wgs2bas = MSc_Matrix3x3::getRotationMatrix( m_att_WGS );
    m_T_bas2wgs = m_T_wgs2bas.getTransposed();

    m_T_wgs2ned = m_location.getT_wgs2ned();
    m_T_ned2wgs = m_location.getT_ned2wgs();

    m_T_ned2bas = m_T_wgs2bas * m_T_ned2wgs;
    m_T_bas2ned = m_T_ned2bas.getTransposed();

//    double roll  = 0.0;    // [rad] roll
//    double pitch = 0.0;    // [rad] pitch
//    double head  = 0.0;    // [rad] heading

//    double sinPitch = -m_T_ned2bas(0,2);
//    double cosPitch = sqrt( 1.0 - min( 1.0, sinPitch*sinPitch ) );

//    pitch = atan2( sinPitch, cosPitch );

//    if ( cosPitch > 0.0 )
//    {
//        roll = atan2(  m_T_ned2bas(1,2) ,  m_T_ned2bas(2,2) );
//        head = atan2(  m_T_ned2bas(0,1) ,  m_T_ned2bas(0,0) );
//    }
//    else
//    {
//        roll = atan2( -m_T_ned2bas(1,0) , -m_T_ned2bas(2,0) );
//        head = 0.0;
//    }
}
